#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  none)
#pragma config(Hubs,  S2, HTServo,  HTServo,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     motorI,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     Arm,           tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     LeftMid,       tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     LeftSides,     tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     RightMid,      tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C3_2,     RightSides,    tmotorTetrix, openLoop, reversed)
#pragma config(Servo,  srvo_S2_C1_1,    goalGrabber,          tServoStandard)
#pragma config(Servo,  srvo_S2_C1_2,    wrist,                tServoStandard)
#pragma config(Servo,  srvo_S2_C1_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_5,    ballCollecter,        tServoStandard)
#pragma config(Servo,  srvo_S2_C1_6,    servo6,               tServoNone)
#pragma config(Servo,  srvo_S2_C2_1,    leftExtender,         tServoContinuousRotation)
#pragma config(Servo,  srvo_S2_C2_2,    rightExtender,        tServoContinuousRotation)
#pragma config(Servo,  srvo_S2_C2_3,    servo9,               tServoNone)
#pragma config(Servo,  srvo_S2_C2_4,    servo10,              tServoNone)
#pragma config(Servo,  srvo_S2_C2_5,    servo11,              tServoNone)
#pragma config(Servo,  srvo_S2_C2_6,    servo12,              tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#define TANK_4M
#include "../library/drive_modes/drive_modes.h"
#include "../library/controllers/tank_controller.c"
#include "includes/init_teleop.c"

// Function Code Includes //
#include "includes/servo_417.c"
#include "includes/motors_417.c"

#pragma debuggerWindows("nxtLCDScreen");
//#pragma debuggerWindows("Servos");
#pragma debuggerWindows("joystickSimple");

#include "JoystickDriver.c"

task main()
{
  RegisterDriveMotors(LeftMid, LeftSides, RightMid, RightSides);
	InitializeTeleOP();

	waitForStart();

	StartTask(DriveTank);
	while(true)
	{
		if (joy2Btn(3) == 1)
		{
			ReleaseGoal();
		}
		else if (joy2Btn(2) == 1)
		{
			GrabGoal();
		}

		//arm raiser
		if (abs(joystick.joy2_y1) > 15)
		{
			SetArmPower(joystick.joy2_y1); // functions called autoattenuate speed between -100 and +100
		}
		else
		{
			StopArmMovement();
		}

		//arm extender
		if (joy2Btn(7) == 1)
		{
			MoveExtenderOut(); // takes optional power input
		}
		else if (joy2Btn(6) == 1)
		{
			MoveExtenderIn(); // takes optional power input
		}
		else
		{
			StopExtender();
		}

		//tube code
		if (joy2Btn(4) == 1)
		{
			ReleaseBalls();
		}
		else if (joy2Btn(5) == 1)
		{
			CollectBalls();
		}
	}
}
